#include "quat.h"
#include "vec.h"
#include <math.h>

Quat4 Quat4::Multiply(const Quat4& qa, const Quat4& qb)
{
	Quat4 q;
	q.x = qa.w*qb.x + qa.x*qb.w + qa.y*qb.z - qa.z*qb.y;
	q.y = qa.w*qb.y + qa.y*qb.w + qa.z*qb.x - qa.x*qb.z;
	q.z = qa.w*qb.z + qa.z*qb.w + qa.x*qb.y - qa.y*qb.x;
	q.w = qa.w*qb.w - qa.x*qb.x - qa.y*qb.y - qa.z*qb.z;
	return q;
}

Quat4 Quat4::Slerp(const Quat4 &qa, const Quat4 &qb, float w)
{
	float cr, sr, rad, t1, t2;
	Quat4 qt, q;

	cr = Quat4::DotProduct(qa, qb);

	if (cr < 0.0f)
	{
		qt.x = -qa.x;
		qt.y = -qa.y;
		qt.z = -qa.z;
		qt.w = -qa.w;
		cr = -cr;
	}
	else
	{
		qt = qa;
	}

	if (1.0f - 0.05f < cr)
	{
		q.x = qt.x + (qb.x - qt.x) * w;
		q.y = qt.y + (qb.y - qt.y) * w;
		q.z = qt.z + (qb.z - qt.z) * w;
		q.w = qt.w + (qb.w - qt.w) * w;
		return q;
	}

	rad	= (float)acosf(cr);
	sr = (float)sinf(rad);

	t1 = (float)sinf(rad * (1.0f - w)) / sr;
	t2 = (float)sinf(rad * w) / sr;

	q.x	= qt.x * t1 + qb.x * t2;
	q.y	= qt.y * t1 + qb.y * t2;
	q.z	= qt.z * t1 + qb.z * t2;
	q.w	= qt.w * t1 + qb.w * t2;
	return q;
}

Quat4 Quat4::MakeFromTo(const Vec3& v1, const Vec3& v2)
{
	Vec3 vn = Vec3::Normalize(v1+v2);
	Vec3 q_xyz = Vec3::CrossProduct(v2, vn);
	float q_w = Vec3::DotProduct(vn, v2);

	Quat4 q;
	q.x = q_xyz.x;
	q.y = q_xyz.y;
	q.z = q_xyz.z;
	q.w = q_w;
	return q;
}

Quat4 Quat4::MakeFromAxisAngle(const Vec3& axis, float rad)
{
	float cs = cos(rad*0.5f);
	float sn = sin(rad*0.5f);
	Quat4 q;
	q.x = axis.x*sn;
	q.y = axis.y*sn;
	q.z = axis.z*sn;
	q.w = cs;
	return q;
}

void Quat4::Decompose(Vec3& axis, float radian) const
{
#ifdef _D3D9
	D3DXQuaternionToAxisAngle( (const D3DXQUATERNION*)this, (D3DXVECTOR3*)&axis.x, &radian );
#endif
}

// Quaternion To Euler
void Quat4::MakeToEuler(const Quat4 &q, float *r)
{
	float sqw = q.w * q.w;
	float sqx = q.x * q.x;
	float sqy = q.y * q.y;
	float sqz = q.z * q.z;

#ifdef WIN32
	r[0] = (float)atan2l(2.0 * ( q.y * q.z + q.x * q.w ) , ( -sqx - sqy + sqz + sqw ));
	r[1] = (float)asinl(-2.0 * ( q.x * q.z - q.y * q.w ));
	r[2] = (float)atan2l(2.0 * ( q.x * q.y + q.z * q.w ) , (  sqx - sqy - sqz + sqw ));
#else
	r[0] = (float)atan2f(2.0 * ( q.y * q.z + q.x * q.w ) , ( -sqx - sqy + sqz + sqw ));
	r[1] = (float)asinf(-2.0 * ( q.x * q.z - q.y * q.w ));
	r[2] = (float)atan2f(2.0 * ( q.x * q.y + q.z * q.w ) , (  sqx - sqy - sqz + sqw ));
#endif
}

// Quaternion From Euler
Quat4 Quat4::MakeFromEuler(float rx, float ry, float rz)
{
	float cr = cos(rx/2);
	float cp = cos(ry/2);
	float cy = cos(rz/2);
	float sr = sin(rx/2);
	float sp = sin(ry/2);
	float sy = sin(rz/2);

	float cpcy = cp * cy;
	float spsy = sp * sy;

	return Quat4(sr * cpcy - cr * spsy, cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy, cr * cpcy + sr * spsy);
}

